#include "StdAfx.h"
#include ".\rangefinderarray.h"
#include <stdlib.h>

RangeFinderArray::RangeFinderArray(void)
{
	m_arrRangeFinderPositions = NULL;
	m_arrRangeFinderDirections = NULL;

	m_arrLfRanges = NULL;

	m_arrXformedPos = NULL;
	m_arrXformedDir = NULL;
	m_nNumRangeFinders = 0;
	m_nNumRangeFindersPerSide = 0;
	m_lfAngleStep = 0.0;
}

RangeFinderArray::~RangeFinderArray(void)
{
	if(m_arrRangeFinderPositions)
	{
		delete [] m_arrRangeFinderPositions;
	}
	if(m_arrRangeFinderDirections)
	{
		delete [] m_arrRangeFinderDirections;
	}
	if(m_arrLfRanges)
	{
		delete [] m_arrLfRanges;
	}
	if(m_arrXformedPos)
	{
		delete [] m_arrXformedPos;
	}
	if(m_arrXformedDir)
	{
		delete [] m_arrXformedDir;
	}
}

void RangeFinderArray::ResetRanges(double lfMaxRange)
{
	for(int i = 0; i < m_nNumRangeFinders; ++i)
	{
		m_arrLfRanges[i] = lfMaxRange;
	}
}

void RangeFinderArray::CollideConvexPoly(const double arrP2PolyBoundary[][2], int nNumPts)
{
	if(nNumPts < 2)
	{
		return;
	}
	for(int i = 0; i < m_nNumRangeFinders; ++i)
	{
		double p2PerpDir[2];
		p2PerpDir[0] = m_arrXformedDir[i][1];
		p2PerpDir[1] = -m_arrXformedDir[i][0];
		
		double p2RelPosCurr[2];
		p2RelPosCurr[0] = arrP2PolyBoundary[nNumPts-1][0] - m_arrXformedPos[i][0];
		p2RelPosCurr[1] = arrP2PolyBoundary[nNumPts-1][1] - m_arrXformedPos[i][1];

		double p2ParrDistCurr = p2RelPosCurr[0]*m_arrXformedDir[i][0] +
			p2RelPosCurr[1]*m_arrXformedDir[i][1];

		double p2PerpDistCurr = p2RelPosCurr[0]*p2PerpDir[0] +
			p2RelPosCurr[1]*p2PerpDir[1];
		for(int nVertex = 0; nVertex < nNumPts; ++nVertex)
		{
			double p2RelPosNext[2];
			p2RelPosNext[0] = arrP2PolyBoundary[nVertex][0] - m_arrXformedPos[i][0];
			p2RelPosNext[1] = arrP2PolyBoundary[nVertex][1] - m_arrXformedPos[i][1];

			double p2ParrDistNext = p2RelPosNext[0]*m_arrXformedDir[i][0] +
				p2RelPosNext[1]*m_arrXformedDir[i][1];

			double p2PerpDistNext = p2RelPosNext[0]*p2PerpDir[0] +
				p2RelPosNext[1]*p2PerpDir[1];


			// collision meat goes here
			if(p2PerpDistCurr*p2PerpDistNext < 0.0)
			{
				// isectPerp = 0
				double lfIsectFraction = fabs(p2PerpDistCurr)/(fabs(p2PerpDistCurr) + fabs(p2PerpDistNext));
				double lfIsectRange = p2ParrDistCurr + lfIsectFraction*(p2ParrDistNext - p2ParrDistCurr);
				if(lfIsectRange >= 0.0 && lfIsectRange < m_arrLfRanges[i])
				{
					m_arrLfRanges[i] = lfIsectRange;
				}
			}
			else if(p2PerpDistCurr == 0.0)
			{
				if(p2ParrDistCurr >= 0.0 && p2ParrDistCurr < m_arrLfRanges[i])
				{
					m_arrLfRanges[i] = p2ParrDistCurr;
				}
			}

			p2ParrDistCurr = p2ParrDistNext;
			p2PerpDistCurr = p2PerpDistNext;
			p2RelPosCurr[0] = p2RelPosNext[0];
			p2RelPosCurr[1] = p2RelPosNext[1];

		}
	}
}

void RangeFinderArray::SetXformedParams(const Pos2d p2BackPoint, const Pos2d p2Dir, const Pos2d p2PerpDir)
{
	for(int i = 0; i < m_nNumRangeFinders; ++i)
	{
		m_arrXformedPos[i][0] = m_arrRangeFinderPositions[i][0] * p2Dir[0] +
			m_arrRangeFinderPositions[i][1] * p2PerpDir[0] + p2BackPoint[0];

		m_arrXformedPos[i][1] = m_arrRangeFinderPositions[i][0] * p2Dir[1] +
			m_arrRangeFinderPositions[i][1] * p2PerpDir[1] + p2BackPoint[1];

		m_arrXformedDir[i][0] = m_arrRangeFinderDirections[i][0] * p2Dir[0] +
			m_arrRangeFinderDirections[i][1] * p2PerpDir[0];

		m_arrXformedDir[i][1] = m_arrRangeFinderDirections[i][0] * p2Dir[1] +
			m_arrRangeFinderDirections[i][1] * p2PerpDir[1];
	}
}

void RangeFinderArray::SetParams(double lfLength, double lfWidth, double lfAngleStep, int nNumPerSide)
{
	m_lfAngleStep = lfAngleStep;
	m_nNumRangeFinders = 2*nNumPerSide;
	m_nNumRangeFindersPerSide = nNumPerSide;
	double lfTotalSweep = (nNumPerSide-1)*lfAngleStep;
	double lfSweepOffset = 0.5*lfTotalSweep;

	if(m_arrRangeFinderDirections)
	{
		delete [] m_arrRangeFinderDirections;
	}
	m_arrRangeFinderDirections = new Pos2d[m_nNumRangeFinders];
	if(m_arrRangeFinderPositions)
	{
		delete [] m_arrRangeFinderPositions;
	}
	m_arrRangeFinderPositions = new Pos2d[m_nNumRangeFinders];
	for(int i = 0; i < nNumPerSide; ++i)
	{
		m_arrRangeFinderDirections[i][0] = cos(i*lfAngleStep-lfSweepOffset);
		m_arrRangeFinderDirections[i][1] = sin(i*lfAngleStep-lfSweepOffset);

		m_arrRangeFinderPositions[i][0] = lfLength+0.5*lfWidth*m_arrRangeFinderDirections[i][0];
		m_arrRangeFinderPositions[i][1] = 0.5*lfWidth*m_arrRangeFinderDirections[i][1];

		m_arrRangeFinderDirections[i+nNumPerSide][0] = -m_arrRangeFinderDirections[i][0];
		m_arrRangeFinderDirections[i+nNumPerSide][1] = -m_arrRangeFinderDirections[i][1];

		m_arrRangeFinderPositions[i+nNumPerSide][0] = lfLength-m_arrRangeFinderPositions[i][0];
		m_arrRangeFinderPositions[i+nNumPerSide][1] = -m_arrRangeFinderPositions[i][1];
	}

	if(m_arrXformedPos)
	{
		delete [] m_arrXformedPos;
	}
	m_arrXformedPos = new Pos2d[m_nNumRangeFinders];

	if(m_arrXformedDir)
	{
		delete [] m_arrXformedDir;
	}
	m_arrXformedDir = new Pos2d[m_nNumRangeFinders];

	if(m_arrLfRanges)
	{
		delete [] m_arrLfRanges;
	}
	m_arrLfRanges = new double[m_nNumRangeFinders];
}
